


• I 



. ^ t. ‘ r "~ rr 

v i V 



■10 



NAVAL POSTGRADUATE SCHOOL 
Monterey , California 




THESIS 

W 597 &OZ 



REAL-TIME IMPLEMENTATION OF AN 
ADAPTIVE DEPTH CONTROLLER 
FOR A SUBMERSIBLE 



by 



James M. Williams 



December 1989 



Thesis Advisor: Roberto Cristi 



Approved for public release; distribution is unlimited 



1248134 



UNCLASSIFIED 

CURITY CLASSIFICATION OF THIS PAGE 



REPORT DOCUMENTATION PAGE 



Form Approved 
OMB No 0704-0188 



3 REPORT SECURITY CLASSIFICATION 

JNCLASSI FIED 



lb RESTRICTIVE MARKINGS 



3 SECURITY CLASSIFICATION AUTHORITY 



D. DECLASSIFICATION /DOWNGRADING SCHEDULE 



3 DISTRIBUTION /AVAILABILITY OF REPORT 

Approved for public release; dis- 
tribution is unlimited 



PERFORMING ORGANIZATION REPORT NUMBER(S) 



5 MONITORING ORGANIZATION REPORT NUMBER(S) 



a. NAME OF PERFORMING ORGANIZATION 



aval Postgraduate School 



6b OFFICE SYMBOL 
(If applicable) 

62 



7a NAME OF MONITORING ORGANIZATION 

Naval Postgraduate School 



c. ADDRESS {City, State, and ZIP Code) 



onterey, California 93943-5000 



7b ADDRESS (Oty, State, and ZIP Code) 



Monterey, California 93943-5000 



a. NAME OF FUNDING / SPONSORING 
ORGANIZATION 



8b OFFICE SYMBOL 
(If applicable) 



9 PROCUREMENT INSTRUMENT IDENTIFICATION NUMBER 



c. ADDRESS (City, State, and ZIP Code) 



10 SOURCE OF FUNDING NUMBERS 



PROGRAM 


PROJECT 


TASK 


WORK UNIT 


ELEMENT NO 


NO 


NO 


ACCESSION NO 



1 TITLE (Include Security Classification) REAL-TIME IMPLEMENTATION OF AN ADAPTIVE DEPTH 
CONTROLLER FOR A SUBMERSIBLE 



2 PERSONAL AUTHOR(S) 

WILLIAMS, James M. 



3a TYPE OF REPORT 

Master's Thesis 



13b TIME COVERED 
FROM TO 



14 DATE OF REPORT (Year, Month, Day) 

1989 December 



15 PAGE COUNT 

86 



6 supplementary notation The views expressed in this thesis are those of the 
author and do not reflect the official policy or position of the Depart- 
ment of Defense or the US Government. 



7 COSATI CODES 


FIELD 


GROUP 


SUB-GROUP 















18 SUBJECT TERMS ( Continue on reverse if necessary and identify by block number) 

AUV ; Adaptive Control; RLS ; Variable Structure; 
Sliding Mode 



9 ABSTRACT (Continue on reverse if necessary and identify by block number) 

An Autonomous Underwater Vehicle (AUV) is an unmanned submersible vehicle capable of 
performing a variety of missions. The AUV, which is the subject of this research, is 
a small prototype vehicle equipped with various control surfaces as well as telemetry 
devices which provide pertinent measurements of the vehicle states. This research is 
directed toward the development and implementation of a digital-control program which 
provides robust depth control of the vehicle. An adaptive parameter estimation routine 
is used to develop the model of the relationship between the actuator commands and 
vehicle response. State feedback is then provided using a variable-structure approach. 
The control algorithm has been implemented through a Turbo Pascal digital-control pro- 
gram executed on a PC/AT computer. Results of the parameter estimation routine and 
controller implementation are discussed. 



20 DISTRIBUTION AVAILABILITY OF ABSTRACT 

@ UNCLASSIFIED/UNLIMITED □ SAME AS RPT □ DTlC USERS 


21 ABSTRACT SECURITY CLASSIFICATION 

UNCLASSIFIED 


22a NAME OF RESPONSIBLE INDIVIDUAL 

CRISTI, Roberto 


22b TELEPHONE (Include Area Code) 

408-646-2223 


22c OFFICE SYMBOL 

62Cx 



>D Form 1473, JUN 86 



Previous editions are obsolete 

S/N 0102-LF-014-6603 



SECURITY CLASSIFICATION OF THIS PAGE 

UNCLASSIFIED 



Approved for public release; distribution is unlimited 



Real-Time Implementation of an Adaptive 
Depth Controller for a Submersible 

by 

James M. Williams 
Lieutenant, United States Navy 
B.O.E., University of Mississippi, 1983 

Submitted in partial fulfillment of the 
requirements for the degree of 

MASTER OF SCIENCE IN ELECTRICAL ENGINEERING 

from the 

NAVAL POSTGRADUATE SCHOOL 
December 1989 



ABSTRACT 



An Autonomous Underwater Vehicle (AUV) is an unmanned 
submersible vehicle capable of performing a variety of 
missions. The AUV, which is the subject of this research, is 
a small prototype vehicle equipped with various control 
surfaces as well as telemetry devices which provide pertinent 
measurements of the vehicle states. This research is directed 
toward the development and implementation of a digital control 
program which provides robust depth control of the vehicle. 
An adaptive parameter estimation routine is used to develop 
the model of the relationship between the actuator commands 
and vehicle response. State feedback is then provided using 
a variable structure approach. The control algorithm has been 
implemented through a Turbo Pascal digital control program 
executed on a PC/AT computer. Results of the parameter 
estimation routine and controller implementation are 
discussed. 
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I. 



INTRODUCTION 



Currently, there is a strong interest within the U.S. Navy 
concerning the development of an Autonomous Underwater Vehicle 
(AUV) . The possible mission objectives of such a vehicle are 
far-ranging, including applications in operations such as ASW, 
and underwater surveillance. The AUV must be capable of 
following a pre-programmed route to a designated target area, 
perform its mission and surface for recovery and subsequent 
data extraction. In response to the Navy's interest in the 
AUV, a great deal of research has been conducted at NPS, 
resulting in the construction of a small-scale prototype 
vehicle. 

This work addresses the real-time implementation of a 
depth controller for the prototype vehicle. In designing a 
depth controller for the AUV, there are a number of associated 
problems which are of notable concern. First of all, the 
equations of motion of the submersible are determined by the 
nonlinear hydrodynamic forces of the vehicle which are 
difficult to approximate by conventional means. Also, the 
dynamics change under varying operating conditions such as 
speed and vehicle configuration. In addition, the measurement 
hardware associated with this particular vehicle produce a 
state measurement which is corrupted with an undesirable time- 
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varying bias. This time-varying component cannot be modeled, 
and must be approximated, then removed from the measured 
signal. The various problems associated with the dynamic 
modeling of the AUV illuminate the need for an adaptive system 
identification approach. In this work, the RLS (Recursive 
Least Squares) algorithm was used to determine the dynamic 
model of the system. The RLS algorithm accepts measurements 
of the system input and output and returns the model 
parameters which, when applied to the model, minimize the 
error between the estimated and measured system output. This 
approach provided an accurate representation of the varying 
dynamics of the system by accounting for the relationship 
between the input and related output of the system under all 
operating conditions. 

Once the system states and parameters have been estimated, 
they are applied to a control law in order to provide the next 
input signal to the system. The controller used in this work 
is known as a Variable Structure (VS) controller . VS controllers 
have recently been proposed for use in the control of 
submersibles and other vehicles with nonlinear or unmodeled 
dynamics [Ref.l]. The variable structure technique 
implemented in conjunction with an adaptive state estimation 
algorithm, guarantees robust trajectory control. The remainder 
of this work addresses the detailed development of the 
f orementioned concepts as well as their physical 
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implementation. A digital-control program will be developed 
which provides a diveplane command to the AUV, based on 
sampled voltage signals representing the states of the 
vehicle. This thesis is organized to represent the logical 
progression of the development of the control program from 
theoretical conception to implementation. Chapter II provides 
a detailed discussion of the unique dynamics associated with 
the AUV. Additionally, a linearized model of vehicle dynamics 
in the vertical plane is developed. Chapter III addresses the 
design of an adaptive state estimation routine, which provides 
estimates of the system states needed for feedback. In 
Chapter IV, the theory supporting the development of the 
variable structure controller is discussed, and the resulting 
controller design is tailored for specific application in the 
AUV depth control system. Chapters V and VI detail the 
implementation of the developed control concepts through 
discussion of the hardware configuration and digital program 
development, respectively. The experimental results of the 
control process are presented in Chapter VII. The results 
provided by the parameter estimation routine and controller 
are segmented to allow for independent analysis. Chapter VIII 
summarizes the performance analysis of the depth control 
system, resulting in the statement of conclusions regarding 
the performance of the developed control process. 
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II. AUV DYNAMICS 



A. GENERAL DESCRIPTION OF THE AUV 

The AUV considered in this research is 30" in length, with 
a rectangular body cross section as depicted in Figure 1 
[Ref. 2]. The vehicle is equipped with twin screws to provide 
propulsion as well as three sets of control surfaces: rudder, 
bow planes, and dive planes. The two rudders operate in 
tandem to control the direction of forward motion of the 
vehicle. The bow planes ensure that the vehicle maintains a 
minimum roll angle via symmetrically opposed operation, while 
the diveplanes control the vehicle's depth. In addition, 



sway 



surge 



pHch 

4 — 



=V 

roll 






yaw 




Figure 1 Sketch of the AUV Model 
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the AUV is equipped with a depth cell which provides depth 
measurements, as well as three rate gyros. The pitchrate, 
rollrate, and yawrate gyros are positioned within the vehicle 
in such a way as to provide for measurement of vehicles 
movement in the pitch, roll, and yaw axis. 

B. DETERMINATION OF THE DEPTH CONTROL MODEL 

Mathematical models based on the equations of motion for 
a body of revolution encompass six degrees of freedom of 
movement referenced to both the body-fixed coordinate system 
and the inertial reference frame [Ref. 3]. The specific 
coordinates are represented in Figure 1 and listed below as 
follows : 



u = surge rate 


x = surge 


v = sway rate 


y = sway 


w = heave rate 


z = heave 


p = roll rate 


p = roll 


q = pitch rate 


6 = pitch 


r = yaw rate 


= yaw . 



The development of the hydrodynamic model is based on the 
Navy's SDV-9 Swimmer Delivery Vehicle, and is detailed in Ref. 
3 and Ref. 4. In designing a depth-control system, we would 
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intuitively be concerned only with depth, pitch, and 
pitchrate. Due to the fact that the AUV is a MIMO (Multi- 
input, Multi-output) system, however, we expect some degree 
of cross-coupling between the different inputs and outputs of 
the system. This cross-coupling effect can be seen, for 

example, when a rudder command is applied to the AUV while 

operating at a non-zero roll angle. In response to the rudder 
command, the vehicle would certainly experience a change in 
depth together with the intended change in direction. In this 
situation, the depth control system would generate and apply 
a signal to the diveplanes in order to maintain the desired 
depth. Schwartz [Ref. 3] addressed this cross-coupling and 
estimated the effect by applying various diveplane, bowplane, 
and rudder commands to the vehicle while observing the effect 
of these commands on all system states. For the particular 
shape of the vehicle considered, analysis of this data led to 
the conclusion that cross-coupling between the system inputs 

and outputs is not of significant concern if the bowplane, 

diveplane, and rudder are operated independently. It is by 
this assumption of independence that we can model the dynamics 
in the vertical plane as a SISO (Single-input, Single-output) 
system. 

C. DERIVATION OF THE MATHEMATICAL MODEL 

Detailed mathematical models of submersibles are available 
from many sources. Basically, they are derived from laws of 
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hydrodynamics and lead to very complex sets of nonlinear 
differential equations. A simpler class of models can be 
obtained by observing the behavior of the state vector under 
several command inputs. In a diving maneuver, the system 
states of importance are pitchrate (Q) , pitch ( 0 ) , and depth 
(Z) , while the command is the diveplane angle ( 6 ). Observing 
the behavior of the vehicle under consideration, the dynamics 
between the diveplane angle ( 8 ), the pitchrate (Q) , and pitch 
( 0 ) are approximated as 

Q(t) = aQ(t) + b5(t) (1) 

6{t) = Q(t) (2) 

while the model for depth (Z) becomes, 



Z(O = -Vsin(0(/)) (3) 

with V being the forward velocity of the vehicle. 

Justification for (3) can be seen by examining the 
proportionality between the velocity of the vehicle and the 
rate of change of depth. The velocity of the vehicle can be 
expressed as a vector of known magnitude, directed at an angle 
0 , relative to the horizontal plane. By simple vector 

analysis, the velocity vector can be resolved into both 
horizontal and vertical components for a given 0 and velocity. 
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The resolvant vertical component of the velocity vector is 
then taken to be the rate of change in depth as in (3) . 

The parameters a and b are dependent on several factors, 
such as operating conditions and velocity. To give a general 
idea of the magnitude of these parameters for small pitch 
angles, (1) through (3) are written in state space form as, 
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with Q in radians/second, 6 in radians, and Z in feet. This 
model applies only to the specific vehicle under study, 
cruising at a velocity of 2.1 feet/second. If the basic 
shape, mass, or velocity of the vehicle changes, the result 
will be a corresponding change in the parameters which 
represent the dynamic model of the system. For this reason, 
the conventional approach to modeling the system dynamics is 
generally considered unsatisfactory. The parameter estimation 
routine implemented in this work is an adaptive scheme which 
accounts for the changing dynamics of the system. This 
adaptive parameter estimation approach is discussed in detail 
in the following chapter. 
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III. 



ESTIMATOR DESIGN 



A. BACKGROUND 

A state estimator, or observer, is one of the fundamental 
elements of any practical feedback-control system. In order 
to stabilize the dynamics of a system, the system states must 
be available for measurement or otherwise be estimated by a 
suitable algorithm. In designing the AUV, certain limitations 
were placed on the selection of sensor hardware which made the 
direct measurement of all system states impossible. Ideally, 
the AUV would be equipped with a position gyro to measure the 
vehicle pitch angle, as well as a pitchrate gyro and depth 
cell to measure vehicle pitchrate and depth. Unfortunately, 
the inclusion of a pitch gyro was impractical due to both the 
unit cost and space requirements for installation. The 
quality of the pitchrate gyro selected was also limited by 
unit cost. The pitchrate gyro selected was a low quality unit 
typically found in hobby shops. As a consequence, the 
pitchrate gyro produces a sensor signal corrupted by a slowly 
varying DC bias voltage. In summary, due to the physical 
limitations of the sensors used, depth is the only state which 
is accurately measureable. 

B. GENERAL DESIGN APPROACH 

To generate estimates of the remaining two states, 
required the design of a state estimator which accepts depth 
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Figure 2 Estimator Design Problem Division 

and corrupted pitchrate measurements as inputs and generates 
accurate estimates of all system states at the output. The 
general approach, depicted in Figure 2, is to use a hybrid 
design encompassing both RLS (Recursive Least Squares) methods 
to estimate the bias embedded in the pitchrate gyro signal, 
as well as LQE (Linear Quadratic Estimator) techniques to 
provide a pitch estimate given an estimate of pitchrate and 
the measured depth of the vehicle. 

C. OVERVIEW OF THE RLS ALGORITHM 

Consider a first-order SISO system with unknown dynamics. 
Let the input sequence to the system be u(t) and the system 
output be y(t) . The plant of such a system can be modeled by 
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a first order difference equation: 



y(t) = ay{t-l) + bu{t-l) 



(5) 



y(f) = [y(f-l) u{t-\)] a h 



( 6 ) 



y(t) = <P T (t-i)e 



( 7 ) 



where is the regression vector containing the history of 
the system inputs and corresponding outputs, and £ is the 
parameter vector containing the system parameters a and b. 
Since the dynamic model of the system is fully describable by 
the parameters a and b, our objective is to estimate the 
parameter vector £ from the available input and output data. 
In order to accomplish this, the system is first modeled in 
state space form as, 



where H T is the regression vector <£ T (t-l) . The term u t in (8) 
accounts for any drifts in the parameters which occur through 
time, and is the Gaussian measurement noise. Let us define 
the autocovariance of u t and v t respectively. 




(8) 



y,=H,e t + v t 



( 9 ) 




(10) 
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R, = E^vf] 



(ID 



The next step is to derive a predictor equation based on 
the system model . The vector £ is known to contain the 
constant parameters a and b. Therefore, the predictor 
equation for £ is, 

(12 > 

Accordingly, the predicted output of the system is determined 
by the product of the transition matrix H T and the predicted 
6 vector. 



In this particular framework, the Kalman filter algorithm 
is a least — squares approach to state estimation. The 
predicted output of the system is computed using (13), and 
then compared to the actual measured output to determine the 
prediction error. The Kalman filter serves to minimize the 
variance of this prediction error through repeated 
applications of the corrector equation. 



L = £-i + K«(y«-y.) 

where , 

§.t ~ . 
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The Kalman gain vector K t is dependent upon the 



reliability of the estimation and is determined as, 



5=1 *L 

R t + <pjp t -i <p t 



(16) 



where, for a system of order n, K t is a (n x 1) vector and P t 
is the (n x n) matrix representing the accuracy of the current 
parameter estimation. As the number of recursions increases, 
the magnitude of the prediction error decreases and the values 
of the P t matrix become correspondingly smaller. The obvious 
result being that as the prediction becomes more accurate, the 
system model equations more closely approximate actual system 
response, resulting in an improved system model. The 
estimation error covariance matrix is computed, then updated 
after each iteration using the following formula [Ref. 5]. 



P = P 

1 i 1 (-i 



5-1 <h flfa-l , r 
R t + < Pl 5-1 <Pl 



(17) 



The initial condition P(0) is the covariance of the error in 
the initial state, and represents the a-priori information 
available concerning the estimated parameters. 

D. PITCHRATE BIAS ESTIMATION 

Application of the RLS algorithm to the particular case 
of estimating a signal with an embedded bias proved to be only 
slightly different than that of the general case. Let us 
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Figure 3 AUV Dive System Representation 



consider the ARMA (Auto-Regressive Moving Average) model 
between the diveplane input and pitchrate, as shown in the 
system representation of Figure 3. Note that the system 
cannot be modeled by (5) in its present form due to the added 
bias. Through application of the principles of the RLS 
algorithm; however, the bias can be accurately estimated. To 
accomplish this, the ARMA model of the difference equation 
from the diveplane input to the pitchrate output is introduced 
as it would be with no bias present, 



Q(0 = “Q ('-i)+ >)«(<-') 



(18) 



where a and rj are the discrete equivalents of the continuous 
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time system parameters a and b. If we let Q (t) =Q m (t) -6 (t) ; 
after some manipulation, the dynamic model in terms of Q m 
becomes , 

Q„ (0 = aQJl - 1) ■ + m - 1) + (P(l) - ap(i - 1)) ( 19 ) 

where B is the is the discrete-time bias term. Since B is 
piecewise constant over a small interval, (19) may be further 
simplified by grouping all terms which are a function of 6 
and replacing them with a single constant term y, as follows. 



Qjt)=aQ m (t-l)+TiS(t-l )+ 7 



( 20 ) 



With this final simplification complete, the system model from 
the diveplane input to the bias corrupted pitchrate output Q m 
is expressed as, 



Q„(0 = [Q.('- 



1 ) 5 ( 1 - 1 ) 1 ] 



a 

n 

y 



( 21 ) 



Note that the £ vector does not contain the bias term B, that 
we wish to estimate. The £ vector, however, does contain the 
parameter y identified earlier as, 

y = P(t)~ aP(t-l) ( 22 ) 



From the estimate of 7, the discrete bias term B must be 
approximated, so that it can be later subtracted from the 
estimated corrupted pitchrate measurement. To explain this 
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process, let us recall that this approach to bias estimation 
was based on the fact that 6 could be approximated as a 
constant term over a small time interval. With this 

assumption in mind, we can approximate that B(t-l) is 
approximately equivalent to B(t) , simplifying the relationship 
to 

y = (l-a)/J(f-l) (23) 



where 7 and a are estimated components of £. The discrete 
bias term B may now be expressed as a function of 7 and a; 



w= 



(1-a) 



(24) 



The RLS algorithm can now be applied in this case exactly as 
demonstrated earlier for the generic case. After each 
iteration of the algorithm, a current estimate of the 
parameter vector £ is generated. Using the estimate of £, 
the corrupted pitchrate and bias term are estimated. The bias 
is then subtracted from the corrupted pitchrate estimate to 
yield a pure pitchrate estimate. [Ref. 5] 



E. LINEAR QUADRATIC ESTIMATOR DESIGN 

The estimator design, to this point, provides only an 
estimate of the pitchrate by removing the bias. In order to 
control the depth of the AUV, all states must be available 
for feedback. To generate estimates of vehicle pitch, let us 
consider the reduced system depicted in Figure 4. The 
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pitchrate signal Q, which is the output of the RLS estimator, 
is treated as an input to the second order system relating 
pitchrate to depth. Throughout the design of the LQE we need 
only to be concerned with this portion of the system. 

To begin the analysis, the reduced system is represented 
in the form, 

~e 

Z 

y = [0 l][f]+v, (26) 



0 

- 2.1 



][z] + IaH 



i o 

0 0 



co, 



(25) 



where « t is the process noise and is the measurement noise, 
each with a zero mean value. Furthermore, let the noise 
covariances be expressed as in (27) and (28) [Ref. 6] ; 
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( 27 ) 



E[a) t ojJ] = S l 



E[v ( v ( r ] = K, 



( 28 ) 



The values of the noise covariances are selected by the 
designer to maximize the performance of the estimator. In 
this design, several combinations of S t and R t were specified 
and the resulting Kalman gains were computed using the 
software package MATLAB. The various sets of Kalman gains 
were then applied to the LQE in order to evaluate the 
performance of the estimator. The Kalman gains which provided 
the best performance were then implemented in the digital 
program through use of the equation. 



In the following chapter, the estimated states provided by the 
state estimator will be used in a nonlinear feedback control 
configuration to generate the closed loop input to the system. 



X = AX + Bu + Ki [ y-cX} 



( 29 ) 



where the estimation error is, 



y-cx. 



( 30 ) 
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IV. VARIABLE STRUCTURE CONTROLLER DESIGN 



A. BACKGROUND 

The dynamics of autonomous underwater vehicles are complex 
and highly nonlinear by nature. The hydrodynamic forces which 
govern the behavior of such a system are dependent on 
parameters such as vehicle speed, acceleration and inertia. 
In addition, there are a variety of unmeasurable disturbances 
which add to the uncertainty of the vehicle response. In 
order to design a robust controller using linear control 
techniques, it would be necessary to devise several system 
models and associated control schemes in order to represent 
the system under the varying operating conditions. For this 
reason, modern linear-control techniques generally prove to 
be too complex and computationally intensive to provide 
adequate control of underwater vehicles. 

One recently developed control methodology which deals 
directly with the control of nonlinear systems is the variable 
structure controller . Using the variable structure approach, 
robust trajectory control is guaranteed despite the presence 
of unmodeled or time-varying system dynamics. The remainder 
of this chapter deals with the development and implementation 
of such a controller. 
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B. VARIABLE STRUCTURE CONTROLLER DEVELOPMENT 



Throughout the development of the variable structure 
controller, the system to be controlled is expressed in state 
space form as, 



where the uncertainty of the system model is expressed as the 
quantity Af(x,£ 0 ). This uncertainty can arise from a variety 
of factors such as the presence of unmodeled dynamics in the 
system. The origin of this uncertainty is of little concern 
in the development of the variable structure controller. In 
the development of the variable structure controller, we 
assume knowledge of an upper bound of the disturbance term 
F ( x , 5 0 ) as, 



with F(x,6 0 ) known for all x and S 0 . On the basis of (31) and 
the known bound on if, a controller can be designed to drive 
the state vector x(t) to within a finite bound around zero. 



the matrix A, corresponding to a marginally stable eigenvalue 
A such that 




(31) 




(32) 



In order to accomplish this, let C T be a left eigenvector of 



C t A = AC r 



(33) 
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By multiplying both sides of (31) by C T and substituting (33), 
we obtain 



C T X = AC r X + C T B(<5 0 + A/(X,<5 0 )) (34) 

Combining terms in (34) results in the assignment of the new 
variables 



<J(0 = C T X(0 (35) 

Y = C t B, (36) 

Additionally, the feedback law selected to generate the closed 
loop diveplane command is 

5 0 = -K{x)Sign(a{t)) (37) 

where K(x) is a positive scalar function, such that K(x)>F(x) . 
Making the substitutions of (35) and (36) into (34), yields 

&(t) = Aa(t)+y(S 0 + A/(X,S 0 )) (38) 

where the diveplane command, S 0 is determined by (37) . With 
the system expressed in the form of (38), it can be proven 
that 

Limo(t) = 0 ( 39 ) 
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where a ( t) represents a linear combination of system states. 
[Ref. 1] Note in (39), that a(t) will approach zero within 
a small bound, as determined by the magnitude of Af. 

Examining the development of the controller at this stage, 
we can demonstrate why this method is generally referred to 
as the sliding mode approach. First, note that (39) implies 
that the state vector x(t) tends toward the surface 



which is a hyperplane in the n-dimensional space of the state. 
The plane of the surface a(t) becomes a switching line as 
shown in Figure 5, and the control law, (37) , assumes 
different polarities according to which half plane the state 
is in. Once the state is on the switching line, it remains 
there by virtue of (39) . By properly selecting the vector C 
we obtain 



To prove this claim, we will examine the derivative of the 
quantity o 2 which is indicative of the slope of a(t) vs. time. 



C T X(t ) = 0 



(40) 



LitnX(t) = 0 , 



(41) 



~(o 2 ( 0) = cr(t)&(t) = <t( 0[M0+ A + r A /] 

2 at 



(42) 
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Figure 5 Representation of Switching Line Behavior 



Applying the feedback law, (37) , and imposing the conditions 
A=0 (marginally stable) and K(x)>||Af|| for all x, we obtain 

a(f)a(f)= <y(f)(-K(r) Sign(<5(f) + A/)) (4 3) 

o(t)6(t) < -K{x)\o{t)\+ o{t)bf . (44) 

Examining (44) in light of the imposed conditions, it is 
apparent that the slope of er 2 (t) vs. time is always negative. 
The most important result of this conclusion is that as the 
states of the system track along the sliding surface, they 
will ultimately converge to zero regardless of which half- 
plane the system is operating in. [Ref. 7] 
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C. VARIABLE STRUCTURE CONTROLLER IMPLEMENTATION 



The controller developed in the previous section assumes 
that the nominal linear model of the system (not accounting 
for disturbances) is 

X = AX + Bu (45) 

where the system is marginally stable. Placing the constraint 
of marginal stability on the system ensures that the sliding 
surface C T x -*■ 0, with C T being a left eigenvector of A. From 
the previous discussion, the states of the system, x(t) , will 
follow the switching line to 0. 

Before applying the variable structure technique to the 
AUV, the system model must be partially compensated as shown 
in Figure 6. To accomplish this, an additional feedback loop 
is implemented to stabilize the dynamics from diveplane 
command to pitch. The value of the feedback coefficient was 
selected to provide two eigenvalues at -0.9 and the remaining 
eigenvalue at the origin, making the system marginally stable. 
Additionally, the depth error (rather than depth) is included 
in the state vector so that the state vector becomes, 
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where Z d is the ordered vehicle depth. 
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Figure 6 Diagram of Original and Partially Compensated System 
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Using the partially compensated model, the variable 
structure controller implementation can be applied to the 
system as previously discussed. Using the MATLAB software 
package, the left eigenvector of the system, corresponding to 
the matrix A of (45) and the eigenvalue 1=0, is computed as 



C r = [0.5556 1.0 0.2143] 

leading to the definition of the signal a( t) , 



o{t) = [0.5556 1.0 0.2143] 



Q(f-1)‘ 

e(t~ 1)J 



(47) 



(48) 



corresponding to the sliding surface. One disadvantage 
associated with using the signum function to generate the 
diveplane command 6 0 is that the controller exhibits excessive 
diveplane chatter once the vehicle has reached its steady- 
state operating depth. To eliminate this problem, the satsign 
function as illustrated in Figure 7 can be used in place of 
the signum function. As a result, when the vehicle is near 
its ordered depth and both pitch and pitchrate are approaching 
0, the controller will enter the linear region of operation 
ranging from -A a to t\a , where Aa is a parameter specified by 
the designer. Outside this linear region, the diveplane 
command saturates as it would using the signum function. A 
large negative value of a will result in a full-scale positive 



26 




Linear 

Region 



Figure 7 Satsignum Function 



diveplane command being asserted with the effect of driving 
a into the linear region of control and ultimately to 0. The 
value of the diveplane command of the original system is 
derived from the augmented diveplane command, S ol through use 
of the equation, 



5 = 5 0 - 0.320 ( 49 ) 

The feedback coefficient of 0.32 in (49) was selected to 
ensure that the poles of the partially compensated system were 
placed at -0.9. 
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The only task remaining before the controller can be 
physically implemented is the selection of the variables K(x) 
and A o in Figure 7, which greatly affect the response 
characteristics of the AUV. These parameters were selected 
through a trial-and-error approach during the testing phase 
of this work. The process used and conclusions regarding the 
selected values will be discussed in the Chapter VII. 
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V. ANALOG SIMULATION 



A. GENERAL 

As previously discussed, the AUV under study is equipped 
with a pitchrate gyro and depth cell to provide measurements 
of the modeled states of the system. The underlying objective 
of this research has been to use telemetry data from the AUV 
to generate a state feedback command in order to control the 
depth trajectory of the vehicle. All procedures to this 
point have been based on the assumption that the required data 
is available for processing by the control program. Contrary 
to this assumption, it was discovered during testing that the 
pitchrate gyro had become inoperative. Repeated attempts were 
made to correct the problem with no results. 

With no other options available, the decision was made to 
simulate the AUV in a diving operation using an analog 
computer. In order to simulate the AUV dynamics on an analog 
computer, the linearized differential equations describing the 
system have been implemented through the use of integrators, 
amplifiers, and attenuating potentiometers. Although this 
method produces a reasonably accurate simulation of the 
vehicle dynamic response, the nonlinear components of vehicle 
motion, such as changes in speed during maneuvering, cannot 
be accounted for. On the other hand, there are many 
substantial advantages associated with the use of an analog 
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simulation. Since all system states are generated by the 
analog computer, they are also available for comparison with 
state estimates generated within the control program. In 
addition, the pitchrate bias can be injected into the 
simulation model as a known quantity rather than as an unknown 
disturbance, allowing for accurate performance analysis of the 
RLS algorithm. 

B. DESCRIPTION OF THE ANALOG SIMULATION 

The analog computer used consisted of an array of 
integrators, amplifiers, and potentiometers as well as a 
multi-position voltmeter which is capable of displaying 
voltage measurements at various system nodes. All amplifiers 
and integrators operate within a voltage range of ±10 volts. 
Integrators must be used in conjunction with an amplifier 
which is capable of providing amplifications of 0.1, 1.0, and 
10.0. Negative feedback between the integrator and its 
associated amplifier provides stabilization. Amplifiers can 
also be used independently when configured to operate as 
either a summing junctions or inverters as required. The 
potentiometers are used as attenuators and provide 
amplification of their inputs only when used in series with 
an amplifier. [Ref. 8] 

In developing the analog simulation of the AUV, the 
linearized equations of motion were programmed into the analog 
computer as depicted in Figure 8. The amplification applied 
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to each integrator has been determined based on the desired 
conversion factor from volts to physical units for the system 
states. To explain how these scaling factors were selected, 
let us begin our analysis at the system output. With the 
maximum output of each amplifier being 10 volts, the 
corresponding depth scale of 1 volt equals 1 foot is assigned, 
allowing for a maximum model depth of 10 feet. Moving 
backwards through the system diagram we divide the unit 
conversion factor by the amplification factor of each 
amplifier and multiply by 10 for each attenuating 

potentiometer. This procedure is repeated to determine the 
conversion from volts to physical units for each state as well 
as the diveplane input. The voltage values of the system 
states are sampled at a frequency of 20 Hz by the A/D (Analog 
to Digital) converter. The previously determined conversion 
from volts to physical units is applied to these quantities 
by the digital control program in order to determine the 
appropriate physical quantity in radians, radians/sec, etc. 
Likewise, the diveplane command is converted to volt units 

prior to being applied to the D/A (Digital-to-Analog) 

* 

converter. 
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Figure 8 Schematic of the Analog Simulation 
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VI. DIGITAL PROGRAM IMPLEMENTATION 



A. GENERAL 

Thus far, the basic principles and theories upon which 
the control system design is based have been addressed. The 
model of the AUV dynamics has been presented and related 
nonlinearities, as well as model uncertainties, have been 
discussed. Additionally, a state estimation methodology was 
identified which addresses the unique parameter identification 
problems associated with the AUV. Finally, a controller 
design has been developed which guarantees robustness in the 
presence of unmodeled nonlinearities in the AUV dynamics. 
Implementation of the design methodology requires a digital 
algorithm which accepts the state measurements and desired 
vehicle depth as inputs and generates the correct diveplane 
command to be applied to the vehicle. 

The programming language selected for use in implementing 
the digital program was Turbo Pascal version 3.0. Although 
more powerful languages are available for this application. 
Turbo Pascal has been selected due to its compatibility with 
existing hardware used in the implementation. The digital 
autopilot program is contained in the Appendix. 
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B. DIGITAL/ ANALOG INTERFACE 

Before addressing the development of the digital control 
program, it is necessary to discuss the inter-relationship 
between the digital controller and the analog simulator. 
Control systems such as the one developed are termed sampled 
data control systems. This distinction arises from the fact that 
a digital controller is used to control a system which is 
operating in continuous time. In the specific case of the 
AUV, the system states are sampled at a predetermined rate 
with their respective values held constant between sample 
intervals. The digital control program must then access these 
sampled values and process them in order to determine the 
correct diveplane command to be applied. This interface 
between the digital computer and the AUV is accomplished 
through the utilization of both hardware and software. 

The data acquisition is implemented using the DT-2801 data 
translation board. The DT-2801 consists of 3 A/D channels and 
1 D/A channel which are accessed by reading data from or 
writing data to a specified register. The sampling frequency 
of the DT-2801 is controlled by an external trigger input 
operating at 20 Hz. The three A/D channels sample and 
digitize the measured system values and stores the binary 
equivalent values in an output register. The D/A channel 
reads the digitized diveplane command from the specified input 
register, converts the binary value to its voltage equivalent 
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and applies the command to the rudders. The DT-2801 board 
is controlled by its associated software routine, PCLAB. 

Since the DT-2801 registers only accepts binary values as 
inputs to its registers, a suitable algorithm must be 
developed to convert binary to decimal for data entering the 
program and decimal to binary for data applied to the D/A 
input registers. Conversion of a physical value to a binary 
value, or vice versa, depends on the number of bits of 
resolution used by the converter. The DT-2801 is a 12-bit 
converter and can, therefore, support 2 12 or 4096 Number of 
Codes (NOC) [Ref. 9]. The maximum bipolar voltage range of 
the vehicle simulator is from ±10 volts. Using this 
configuration, a voltage of -10 volts corresponds to a NOC of 
0, and a voltage of 10 volts corresponds to a NOC of 4096. 
The algorithm used to convert between digital and decimal 
values is contained in Ref. 9, PCLAB Users Manual. The 
implementation of the conversion algorithms is contained in 
procedures "GET DIGITAL SENSORY DATA" and "GENERATE DIVEPLANE 
COMMAND" in the digital control program. 

C. DIGITAL PROGRAM DEVELOPMENT 

The digital control program represents the innermost 
functional level of the digital autopilot echelon. All other 
components of the AUV controller serve only to provide 
information to, or apply information from, the digital control 
program. The digital program consists of several integrated 
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modules called procedures, which function as either utilities 
or provide application specific functions. Fortunately, past 
research contained in Ref. 10, has resulted in the development 
of all utility procedures encompassing data management, user 
interface, as well as program flow. These various procedures 
do not address the specific application of the controller 
under development and are not discussed in this paper. The 
focus of the remainder of this chapter will, therefore, be the 
development of the procedures "EST" and "GENERATE DIVEPLANE 
COMMAND", which address the implementation of the specific 
state estimator and controller designs. 

1. Procedure EST 

The procedure EST provides an estimate of the system 
states using a combination of the RLS algorithm and LQE as 
discussed in Chapter II. The main body of EST predicts and 
removes the bias from the pitchrate signal and applies the 
pure pitchrate signal at the input of the LQE. To accomplish 
this, EST employs a number of subordinate procedures, each 
providing a specific function. The following paragraphs 
describe the structure and use of each of the subordinate 
procedures. 

a. Procedure INITIALIZE ARRAYS 

This procedure initializes all system variables 
on the first call to the procedure EST. This is accomplished 
by using a count variable in the main program. At the first 
iteration of the program, the count variable is set to 1 
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before the procedure EST is called. The program branches to 
INITIALIZE ARRAYS, and the count is incremented upon return 
to EST. 

b. Procedure FILTER 

This procedure provides lowpass filtering for both 
the measured pitchrate and the applied diveplane command 
before entering the RLS algorithm. Filtering the input and 
output data reduces the high frequency components of the 
signals which results in a greater speed of convergence of 
the RLS algorithm. Filtering both the input and output data 
has no effect on the accuracy of parameter estimation. 

c . Procedure UPDATE 

UPDATE is used to form the new regression vector 
of input and output data. As the newest sample values are 
read, they are placed in the vector <j > , and the current values 
are deleted. This vector <j> is later used in predicting the 
next set of outputs from the RLS algorithm. 

d . Procedure KGAIN 

This procedure calculates the Kalman gain matrix 
which is to be applied in order to minimize the variance of 
the estimation error. In order to accomplish this, KGAIN 
accepts as inputs, the regression vector <j > , the past error 
covariance matrix P, and the previously calculated Kalman gain 
matrix K. In the process, the matrix P is updated to reflect 
the degree of estimation error present. As the estimation 
process is repeated, the values of the P matrix decrease which 
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results in smaller Kalman gains being applied to the corrector 
equation. 

e . Procedure NEWEST 

NEWEST essentially implements the corrector 
equation resulting in a new predicted parameter vector £. The 
prediction error, or innovation, is first calculated and the 
Kalman gains are applied to this quantity. The past 
prediction of the parameter vector is then updated by the 
calculated correction value. The K matrix, provided by the 
procedure KGAIN, minimizes the error between the predicted 
output and actual output. After repeated application of the 
corrector equation, the prediction error approaches 0 and the 
parameter vector 6_ approaches the correct steady state values. 

f . Procedure INNERPROD 

This procedure produces the current output 
estimate through the product of the regression vector <j> and 
the predicted parameter vector £. 

2. Procedure GENERATE DIVEPLANE COMMAND 

The procedure GENERATE DIVEPLANE COMMAND calculates 
the diveplane command using variable structure controller 
technique. The required code is short and simple, requiring 
little explanation. This procedure calculates the scalar 
value a through the vector product of the left eigenvector of 
the system and the estimated state vector. The diveplane 
command for the augmented system, 6 0 , is then generated by 
applying a scaling factor to satsign(a). The satsign function 
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is implemented using the subordinate program function "SAT". 
Applying the relationship, S = 6 0 - 0.32 6, results in the 
diveplane command for the original system. At this point, a 
software limit of 0.4 radians is placed on the diveplane 
command in order not to exceed the physical travel limit of 
the diveplane actuators. The limited value of 6 is then 
converted from radians to a voltage equivalent. The 
conversion from the decimal voltage value to the binary 
equivalent is performed by another subordinate program 
function, "CONVERT ANALOG 2 DIGITAL". Finally, the binary 
equivalent of the diveplane command is stored in the D/A 
channel 0 input register before being applied to the vehicle 
diveplane . 

D. DESCRIPTION OF PROGRAM FLOW 

The modular design approach of using separate procedures 
and functions to implement the digital autopilot program 
significantly simplifies the design process. However, 
evaluating procedural dependencies and program progression are 
made more difficult by this approach. Figure 9 illustrates 
the conceptual flow of the digital autopilot program. 
Although not all procedures are included, those which are 
crucial to understanding the operation of the control system 
are labeled in Figure 9, and their functions described below. 

• GET TARGET DEPTH - Upon entering the program control 
loop, the user is prompted to enter the desired target 
depth of the vehicle in feet. The desired depth is then 
used to calculate depth error. 
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Figure 9 Conceptual Flow of Digital Control Program 



• RUNMODE SCREEN - Writes vehicle parameters and lists user 
options on the screen. Information is updated at 
periodic intervals selected by the programmer. 

• GET DIGITAL SENSORY DATA - Reads digitized sensory data 
from A/D data registers. Data is converted to physical 
units and stored for processing by the control program. 
The process is coordinated by PC-LAB software. 

• GENERATE DIVEPLANE COMMAND - Computes diveplane command, 
digitizes this value, and stores result in D/A input 
register. 

• EST - Provides prediction of system states. 
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VII. EXPERIMENTAL RESULTS 



A. GENERAL 

This work has been concerned with the theory and 
application of various control concepts which have resulted 
in the development of an integrated state estimation and 
control methodology for the AUV. Throughout the development 
of the estimator and controller, a number of variables which 
have an effect on controller performance have been left 
undetermined. This chapter details the process which lead to 
the selection of these variables as well as the final result 
of depth controller implementation. 

B. RLS ALGORITHM RESULTS 

The RLS algorithm was designed to provide crucial 
information pertaining to the pitchrate of the vehicle. With 
no pitch measurement available, the determination of vehicle 
pitch was also dependent on the RLS algorithm. To evaluate 
the performance of the RLS algorithm, the analog simulator was 
programmed with the continuous time coefficients which express 
the lag relationship between diveplane command and pitchrate. 
The values of the zero and pole were 2.53 and 1.8 respectively 
(representing vehicle dynamics at 2.1 ft/sec). The discrete 
estimates of the lag coefficients generated by the RLS 
algorithm are the first two elements of the parameter vector 
i_. By comparing these estimates to the corresponding discrete 
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equivalents of the lag filter coefficients, the performance 
of the RLS algorithm was examined. Figure 10 represents the 
plot of the estimated discrete equivalent of the pole of the 
model. Note that the value converges to 0.9 in approximately 
four seconds. The correct discrete value of the pole is 
0.9143. The RLS algorithm converges to an extremely accurate 
estimate of the system pole in a reasonable amount of time. 
The estimated discrete equivalent of the zero, as shown in 
Figure 11, converges to a value of 0.139 in approximately 
seven seconds. The correct discrete value of the zero is 
0.127. The estimate of the zero is not as accurate as that 
of the pole, which is generally the case for the RLS 
algorithm. 

Although the approximation of the discrete system 
parameters may prove to be important in the determining the 
dynamic model of future vehicle designs, the most important 
objective in this work is the determination of the bias 
component of the pitchrate signal. The bias was simulated as 
a DC voltage added to the pitchrate voltage output of the 
analog simulator. Figure 12 represents the bias estimate for 
the system with an injected bias of 0.2 volts. The RLS 
algorithm produced a bias estimate which converged to the 
correct value of 0.2 in approximately eight seconds. 
Different values of pitchrate bias, ranging from one to three 
volts, were injected into the simulation and the corresponding 
bias estimate was observed. Varying the magnitude of the 



42 



ESTIMATE OF DISCRETE SYSTEM POLE 




HQnilNOVPJ 



Figure 10 RLS Estimate of Pole 
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Figure 11 RLS Estimate of Zero 
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Figure 12 RLS Estimate of Bias 
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pitchrate bias produced no significant variations in the 
accuracy or convergence time of the algorithm. 

After estimating the pitchrate bias, it was removed from 
the measured pitchrate signal in order to yield a pure signal 
for state feedback as well as input to the LQE. Figure 13 is 
a plot of the measured pitchrate and the estimated pitchrate 
with the bias removed. As is evident from the plot, the bias 
used in this case was 0.2 radians/second. The results clearly 
demonstrate that this approach effectively removes the bias 
from the measured pitchrate signal. 

C. RESULTS OF LQE IMPLEMENTATION 

Design and implementation of the Linear Quadratic 
Estimator was intended to be the least difficult portion of 
the estimator design. Upon experimentation with the 
controller design, it was discovered that this was not the 
case. As previously mentioned, the feedback gains applied to 
the pitch and depth estimates were determined by the ratio of 
the covariance of system noise to measurement noise specified 
by the designer. In this application, an increase in this 
ratio suggests less confidence in the pitchrate input to the 
LQE rather than the measured output (depth) of the vehicle. 
The specified noise covariances do not represent the actual 
noise present in the corresponding signals, but only providea 
means of determining the optimal steady-state Kalman gains to 
be applied to the observer. The general approach was to 
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Figure 13 Pitchrate with Bias vs. Pitchrate with Bias Removed 
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generate several sets of Kalman gains associated with 
different noise covariance combinations and implement the 
gains which provided the best result. The results achieved 
through this appoach were less than ideal. Selecting a small 
ratio between the input noise covariance (S t ) and measurement 
noise covariances (R t ) resulted in an observer which was 
incapable of tracking the pitch of the vehicle. As the noise 
ratio was increased, the tracking performance of the observer 
increased substantially; however, the observer became too 
sensitive to estimation error. The effect of varying the 
ratio between S t and R t is demonstrated in Figure 14 and Figure 
15. In Figure 14, S t and R t were specified to be 6.0 and 1.0, 
respectively. Note that the response of the observer is too 
slow to provide for accurate tracking of the vehicle pitch. 
In Figure 15, S t is 100.0 and R t is 1.0, resulting in increased 
magnitudes of the corresponding feedback gains. As can be 
seen from this result, the increased magnitude of the Kalman 
gains yielded a faster observer which tracked pitch 
considerably better than the case represented in Figure 14 . 
On the other hand, the negative effect of the increased gains 
on the performance of the observer were equally obvious. As 
the gains were increased, the observer became extremely 
sensitive to any error between the estimated depth and the 
measured depth of the vehicle. Although this error is 
minimal, it is amplified by the feedback gain applied to the 
observer resulting in a noisy pitch estimate. However, it was 



48 




49 



PITCH vs. ESTIMATED PITCH 




(SNViavy) HDlId 



.Figure 15 Pitch Estimate with S t 100.0 and Rt = 1.0 
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determined through further experimentation that the 
sensitivity of the observer had little affect on the depth 
trajectory of the vehicle. As a consequence, the case 
represented in Figure 15 was selected in the final 
implementation of the state estimator. 

D. VARIABLE STRUCTURE CONTROLLER RESULTS 

Implementation of the variable structure controller 
involved extensive experimentation in order to determine the 
best combination of selectable controller parameters. The 
parameters in question are the saturation value, K, of the 
satsignum function and the width of the linear region of 
operation, A a. Since the diveplane command is ultimately 
limited to 0.4 radians by physical considerations, varying K 
has little effect on the diveplane command applied to the 
vehicle. The value of K was set to a value of two for the 
controller implementation. In contrast, the width of the 
linear region of operation greatly affects the response 
characteristics of the controller. Selection of a small value 
of Act results in a more rigid controller characterized by 
longer periods of diveplane saturation as well as diveplane 
oscillations as the vehicle maintains the commanded depth. 
The effect on the vehicle depth trajectory was an overshoot 
of the desired depth followed by repeated corrections. Once 
attained, the commanded depth was maintained at the expense 
of continuous diveplane commands. In contrast, large values 
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of A a resulted in a sluggish vehicle response. The diveplanes 
rarely entered the saturation region and therefore, the 
vehicle was slow to reach the desired depth. Once at the 
ordered operating depth, the vehicle was unable to maintain 
depth within a reasonable degree of accuracy. The optimal 
value of Act was obtained through experimentation with 
different values of Act and evaluation of the corresponding 
results. The underlying objective of the controller 
implementation was to provide a robust trajectory to the 
desired depth and maintain that depth with minimum diveplane 
command applied. Figure 16 depicts the plot of the diveplane 
command generated for a dive to five feet using 1.0 as the 
selected value of Act. The corresponding depth trajectory is 
contained in Figure 17. Note that the diveplane is initially 
saturated directing the vehicle toward the desired depth. As 
the vehicle approaches the desired depth, the diveplane 
command provides corrections to the vehicle trajectory. Once 
achieved, the ordered depth was maintained with minimal 
control input. The controller provided the best response with 
Act set at 1.0 and, as a result, this configuration was used 
in the final controller implementation. 
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Figure 17 Depth Trajectory for a Dive to 5 Feet 
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VIII. CONCLUSIONS 



The objective of this work has been to design a digital 
controller which provides robust depth control of an AUV. The 
success or failure of this research is based on the ability 
of the design to provide accurate estimates of the system 
states, and generate a closed-loop control input which is a 
function of the states. Although the digital control program 
was tested using an analog simulation rather than the intended 
prototype vehicle, the results obtained provide for a 
reasonably accurate analysis of the design concepts 
implemented. 

Having only specified the order of the linear model of 
the system, the RLS algorithm estimated the model parameters 
within a reasonable degree of accuracy and provided an equally 
accurate estimate of pitchrate bias. Unfortunately, the 
varying dynamics of the AUV could not be simulated using the 
analog computer. As a result, the performance of the RLS 
algorithm could not be evaluated under the conditions of 
changing speed and acceleration which the vehicle is subjected 
to during maneuvering. From the results obtained, however, 
it would be reasonable to conclude that the algorithm would 
perform satisfactorily under these varying conditions. 
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The design of the pitch estimator was intended to be 
simple while still providing a reasonable estimate of vehicle 
pitch. As discussed in Chapter VII, the LQE did not perform 
as originally intended. The estimator proved to be very 
sensitive to measurement noise, resulting in a noisy pitch 
estimate. The negative effect of the noisy pitch estimate on 
system performance was realized as high frequency oscillations 
in the diveplane command. Fortunately, the effect on the 
resulting depth trajectory of the vehicle was minimal. 

In the final analysis, the combination of an adaptive 
parameter estimation technique and variable structure control 
provided an effective means of vehicle depth control. Even 
in the presence of considerable state estimation error 
produced by the LQE, the variable structure controller 
directed the vehicle along a stable trajectory to the desired 
depth. Due to the flexibility of the controller design, 
virtually any vehicle response characteristic can be obtained 
by altering the controller parameters. Considering the wide 
range of possible missions of the AUV, this particular 
attribute of the variable structure controller might prove to 
be useful. 
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APPENDIX 



DIGITAL CONTROL PROGRAM 



<$c-} 

program AuvAutoPilot ( input, output ) ; 

{ TITLE : AUV Automatic Control Program (AUTOCON3 . PAS) 

AUTHOR : J.M. WILLIAMS 

APPLICATION : Test of real-time controller for the AUV 
DATE : 29 Aug 1989 

Project Description : This program implements digital 
control of the NPS autonomous underwater vehicle (AUV) in 
the vertical or dive plane. It samples vehicle sensor input 
from three channels : depth, pitch, pitchrate. The depth 
signal is then passed to a DepthError module which compares 
the actual sensor depth with a model reference depth 
simulated by a depthgain. An depth error voltage is then 
generated and passed to a Genera teDivePlaneCommand module 
which processes the error signal and sends out an 
appropriate command to the diveplane actuators. The gains in 
the algorithms reflect the discrete transfer function gains 
for diveplane command response derived from vehicle 
identification analysis. ) 



{ GLOBAL DECLARATIONS } 

const 



{ 



Screen declarations 



) 



xl 


= 5; 


yi 


= 2; 


X2 


= 75; 


y 2 


= 24; 



( Upper left corner : left edge ) 
( Upper left corner : upper edge) 
{ Lower right corner : right edge) 

{ Lower right corner : bottom edge) 



type 

strlO = string [10]; 
str60 = string [60]; 

var 

hr , hr2 , min , min2 , 

sec, sec2 ,hun,hun2 : byte; 

seconds : real; 
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{ 



INCLUDED FILES Declarations 



} 



{$1 pcldefs.tp } {PC LAB Trubo Pascal routines.) 

{$1 pclerrs.pas ) {PC LAB error code messages file.) 

{ $Iinidac. auv ) 

{ This procedure initializes the DT 2801-A TO ZERO VOLTS AND 
MUST BE EXECUTED BEFORE THE AUV IS HOOKED TO THE COMPUTER. ) 

{ $Igettime.auv ) 

{ No arguments; returns hr, min, sec, hun ; byte) 
{$Ishotmdff .auv ) 

{ Input the output of TimeDiff.auv and this procedure 
displays the time difference between the two most current 
Get Time. auv results. 

ShowTimeDif ferene ( x: integer) . ) 

{$Itimedif f .auv) 

{ Input; hr,hr2,min,min2,sec,sec2,hun,hun2 from two calls 
of GetTime.auv and this returns the difference in 
seconds as a REAL variable.) 

{ $Idrawbox2 . auv) 

{ Input xl,yl,x2,y2 : integer to specify the corner limits 
of the box. This procedure clears screen and draws a 
rectangular box of specified dimension using ASCII double 
line characters. 

{ $Iclrbox2 .auv) 

{ Input xl,yl,x2,y2 : integer to specify the corner limits 
of the box. This procedure uses a FAST means of clearing a 
box of specified dimension. The box dimension should be 
delcared as constants.) 

{$Iboxprint.auv ) 

{ Input the printrow, leftboxedge, rightboxedge : integer 
and printstring :str60. This procedure centerprints the 
string in the box at the printrow specified without 
overwriting the box border. ) 



{ $Ishowfast .auv) 

{ Input message : str60, column, row ; integer. To specify 
the x,y position on the screen for a FAST message print.) 

{ $Ikeyhit .auv) 

{ This is a boolean function which returns true or false if 
key is pressed; it also returns keycode replies VAR reply. 
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($Itabxy.auv) 

{ Input tabcol , tabrow : integer; like gotoxy) 

{ Iboxpause . auv } 

{ Input xpause , ypause ; integer to specify where "Press any 
key to continue" message is to be printed. } 

{ $Igetkey . auv } 

{ Input as a string of chars, the set of acceptable replies; 
ie 7 YyNn 7 . This procedure waits until one of the acceptable 
replies has been entered. } 

{$Iutils.auv } 

{ Included are some housekeepping and debugging routines. } 

{ $Iconvadv . auv } 

{ Includes functions to convert depth, speed and pitchrate to 
vehicle values. } 

{ ************* MAIN PROGRAMS PROCEDURES *************** } 



{ ************* USER INTERFACE MODULES **************** } 
procedure MainMenu ( var reply : char ) ; 



{ This procedure presents the AUV screen and solicits an 
option to Run the AUV from the StatusAndCommand procedure or 
to Quit. } 



begin 

repeat 
clrscr ; 

drawbox2 (xl,yl,x2,y2) ; 
boxprint (yl+3 , xl , x2 , 7 N A V A 
E SCHOOL 7 ); 
boxprint (yl+5,xl,x2, 7 D E P A 
boxprint (yl+6,xl,x2, 7 M E C H 



L 

R T 
A N 



P 0 S T G R A 

M E N T OF 

I C A L E N 



D U A T 
') ; 

G I N E 



ERING 7 ),* 

boxprint ( yl+8 , xl , x2 , 7 AUTONOMOUS UNDERWATER 

VEHICLE 7 ) ; 

boxprint (yl+10 , xl , x2 , 7 DIGITAL AUTOPILOT CONTROL 

PROGRAM 7 ) ; 

boxprint (yl+12 , xl , x2 , 7 ************************** 7 ) ; 

boxprint (yl+15,xl,x2 , 7 Do You want to RUN this program 
• • ' ) ; 

boxprint (yl+16,xl,x2 , 7 or Do You want to QUIT and return 
to DOS ? 7 ) ; 

boxprint (yl+20,xl,x2, 7 »» ENTER Q OR R «« 7 ); 

getkey ( 7 QqRr 7 , reply , reply2 ) ; 
until ( reply in [ 7 Q 7 , 'q 7 , 7 R 7 , 7 r 7 ] ) and (reply2 = 
chr ( 0 ) ) ; 
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end; 



procedure StatusAndCommand ( var mode : char ) ; 

{ This procedure begins the control program screen. } 

var 

mode2 : char; 

procedure StatusAndCommandScreen; 

{ This is the status and control screen and solicits a user 
input of FI to RUN the program or Q to Quit and exit to the 
main menu. } 

{ StatusAndCommandScreen } 

begin 



clrbox2 (xl,yl,x2,y2) ; 

boxprint (yl+1 , xl , x2 , 7 AUV STATUS / COMMAND AND CONTROL 
SCREEN') ; 

boxprint (yl+2 , xl , x2 , ' — = — - -== === ============ < ) • 

boxprint (yl+7,xl,x2, 'CHOOSE YOUR DESIRED CONTROL MODE 
:') ; 

boxprint (y 1+9, xl,x2, 'ENTER KEY « FI » TO START 
AUV CONTROL') ; 

boxprint ( y 1+1 1 , xl , x2 , ' ENTER « Q » TO QUIT AND 
RETURN TO MAIN MENU') ; 

boxprint (y 1+16 , xl , x2 , ' PRESS EITHER FI OR Q'); 
end; { StatusAndCommandScreen } 



{ ********** CLOSED LOOP CONTROL ROUTINES **************** } 



Procedure ClosedLoopControl ; 

{ This module comprises the closed loop control scheme. } 



label 5 ; 
label 1 ; 
const 

maxdepth = 33; 

mindepth = 0 ; 

updateincrement =10; 



type 

try = array [1.. 10] of real; 

activecontrolmode = ( run, reset, exit ) ; 
allowabledepthrange = mindepth. .maxdepth ; 
auvattitude = ( climb, maintain, diving ) ; 

digital integerarray = array [1..3] of integer; 



var 

filename : string [ 14 ] ; 
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f ilevar :text; 



auvdepth , auvdepthvolts , auvspeed , auvpitch , yawrate , diveplane , 
auvspeedvolts, auvpitchrate, auvpitchratevolts , estdepth, err , 
acl , ac2 , ac3 , ac4 , ac5 , ac6 , deptherrorvolts , targetdepthvolts , 
biasl,divevolts, tgtnew,targetdepth, speed: real ; 



adv 

j , status, time 
modereply , modereply2 
activemode 

updatecounter, initial 

depthrange 

attitude 



: digital integerarray; 

: integer ; 

: char ; 

: activecontrolmode ; 

: integer ; 

: allowabledepthrange; 
: auvattitude; 



procedure GetTargetDepth (var tgtdepth : real ; 

var tgtdepthvolts : real ) ; 

{ This procedure solicits the target AUV operating depth and 
converts it to an AUV equivalent targetdepth analog voltage 
and passes both of these parameters. } 



begin { GetTargetDepth } 

clrbox2 (xl,yl,x2,y2) ; 

boxprint (yl+10 , xl , x2 , ' ENTER THE AUV TARGET OPERATING 
DEPTH' ) ; 

boxpr int(y 1+1 l,xl,x2, 'NOTE : THE DEPTH SHOULD BE IN FEET 

( 0.0 - 10 . 0 ) ') ; 

repeat 

begin 

boxprint (yl+13 , X1,X2 , 'ENTER THE TARGET OPERATING 
DEPTH ' ) ; 

gotoxy (xl+33 ,yl+15) ; 
read ( tgtdepth ) ; 
end; 

until tgtdepth >= 0.0 ; 

tgtdepthvolts : = tgtdepth ; 

end; ( GetTargetDepth ) 

procedure RunModeScreen ; 

{ This procedure displays the Closed Loop Control Screen in 
the RUN MODE. } 

begin { RunModeScreen ) 

clrbox2 (xl,yl,x2 / y2) ; 

boxprint (yl+1 , xl , x2 , ' A UV STATUS / CONT 
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ROL SCREEN'); 

boxprint (yl+2,xl,x2, ' ================ - = --- — • 

boxprint(yl+4,xl,x2, 'STATUS OF A U V OPERATING 
PARAMETERS : ' ) ; 

write (tabxy (xl+5,yl+6) , 'AUV DEPTH [feet] : '); 

write (tabxy ( xl+5,y 1+7) , 'AUV PITCH [rads] : '); 

write (tabxy (xl+5,yl+8) , 'AUV PITCHRATE [rads/sec]: 

') ; 

write (tabxy (xl+5,yl+9) , 'AUVDIVEPLANE [rads] : '); 

write (tabxy (xl+5,yl+10) , 'BIAS = :'); 

boxprint (y 1+11, xl,x2 , 'A U V CONTROL STATUS :'); 
write (tabxy ( xl+5,y 1+13) , 'CURRENT TARGET DEPTH : '); 

write (tabxy (xl+5,yl+14) , 'CURRENT MODE : '); 

write (tabxy (xl+5,yl+15) , 'CURRENT MANEUVER : '); 

boxprint (yl+18 , xl, x2 , 'PRESS KEY FI .. TO ENTER NEW 

TARGET DEPTH. '); 

boxprint (yl+19 , xl , x2 , ' PRESS KEY F3 .. TO EXIT ACTIVE 
CONTROL. ' ) ; 

end; { RunModeScreen } 

procedure 

UpdateRunModeScreen ( updatedepth , updatepitch , updatepitchrate , 
updatediveplane : real ; 

updatetargetdepth : real ; 

updatemode : activecontrolmode ; 
updateattitude : auvattitude ; 
var biasl: real) ; 

( This module updates the Closed Loop Control Run Mode 
Screen with updated display parameters. Updates occur in 
intervals specified by update increment interval declared in 
ClosedLoopControl procedure. } 

begin { UpdateRunModeScreen ) 

{ UPDATES STATUS OF A U V OPERATING PARAMETERS ) 
writeln (tabxy (xl+37 ,yl+6) , updatedepth : 6 : 3) ; 
writeln (tabxy (xl+37, yl+7) ,updatepitch: 6 : 3) ; 
writeln (tabxy (xl+37, yl+8) , updatepitchrate: 6:3) ; 
writeln (tabxy (xl+37 ,yl+9) , updatediveplane: 6: 3) ; 
writeln (tabxy (xl+37 ,yl+10) , biasl: 8 : 6) ; 

{ UPDATES THE AUV CONTROL STATUS } 

write (tabxy (xl+30 ,yl+13) , updatetargetdepth: 6 : 2) ; 
case updatemode of 

run : writeln (tabxy ( xl+30, y 1+14 ), 'RUN '); 
exit : writeln (tabxy (xl+30, yl+14) , 'EXIT '); 
end; 

case updateattitude of 

maintain : writeln (tabxy (xl+30, yl+15) , 'MAINTAINING 

DEPTH ' ) ; 
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climb 



diving 

end; 



writeln (tabxy (xl+30,yl+15) , 'CLIMBING TO 
TARGET DEPTH ' ) ; 

writeln (tabxy (xl+30,yl+15) , 'DIVING TO 
TARGET DEPTH ' ) ; 



end ; { UpdateRunModeScreen } 

procedure GetDigitalSensoryData (var 
depthanalogvolts , pitchanalogvolts , 

pitchrateanalogvolts ;real) ; 

{ This procedure uses PCLAB routines to sample selected 
input telemetry channels from the AUV and digitizes these 
inputs and multiplies them by the specified gains. 

DT 2801-A / DT 707 Board set up: 

channel 1 - AUV depth input 

channel 2 - AUV pitch input 

channel 3 - AUV pitchrate input) 



const 



{ These are artificial gains used to simulate AUV telemetry 
during program development. One signal from a signal 
generator (+/- 1.25, 9 Hz, characteristic of the pitchrate 
signal) is input to all 3 input channels. Gains are applied 
to simulate the actual values. These and their application 
in the procedure body should be removed after program 
development is completed. } 

depthgain = 1.0; 

pitchgain = 1.0; 

pitchrategain = 1.0; 



{ These are AUV to DT 2801-A / DT 707 hook up board channel 
configurations, conversion and computational arguments.) 



depthchanne 1 

depthpfs 

depthmfs 



1; { AUV output to DT— 707 input channel 

assignment ) 

+10.0; { Peak depth signal value) 

-10.0; ( Minimum depth signal value) 



pitchchannel = 2 ; 

spdpfs = +10.0; 

spdmfs = -10.0; 

pitchratechannel = 3 ; 

pitchratepfs = +10.0; 



( AUV output to DT-707 input channel 
assignment ) 

{ Peak speed signal value) 

{ Minimum speed signal value) 

{ AUV output to DT-707 input channel 
assignment } 

{ Peak pitchrate signal value) 
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pitchratemfs = -10.0; { Minimum pitchrate signal value} 

noc = 4096; { Number of Codes; conversion 

resolution. The DT 2801-A performs 
a 12 bit conversion. NOC = (2 A 12 
conversion bits) , ie 4096} 

{ SetUpAdc and ADConTrigger PCL function arguments 

: p 6-8 PCL documentation } 



boardnum 

numa2dchan 

timingsource 

adcgainl 

adcgain2 

adcgain4 

adcgain8 

startchannel 

endchannel 



1 ; 

3; 

2; { — Sets trigger , internal clock } 

1 ; 

2 ; 

4; { Sets the A/D gain; 1} 

8 ; 

1 ; 

3; 



var 

pitchadv, 
depthadv , 
pitchrateadv , 
signaladv, 
counter , status , 
chanum , i , j 



{ Signal analog data value) 
; integer ; 



begin { procedure GetDigitalTelemetry } 

{ Set up the DT 2801-A board to take data.} 

status ;= SelectBoard (boardnum) ; 

{ Set up the DT 2801-A board to take data from 3 input 
channels; Data sampling is initiated by the ADConTrigger 
single channel sample of the depth channel and then single 
ADC value samples of the speed and pitchrate follow. The 
Trigger is connected to the DT 707 board at terminal 49 from 
a signal generating source. } 

status := ADConTrigger ( depthchannel , adcgainl, depthadv ) ; 
status := ADCValue ( pitchchannel, adcgainl, pitchadv ) ; 
status ;= ADCValue ( pitchratechannel, adcgainl, 

pitchrateadv) ; 



{ Convert the digitized Analog Data Values for speed, depth, 
pitchrate to analog voltage values. The algorithm for this 
conversion is found in Appendix D of the PCLAB 
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documentation. } 



depthanalogvolts := ( depthadv * (depthpfs-depthmfs)/noc ) 

+ depthmfs; 

{this allows for a -1 volt bias in the depth cell reading 
at zero depth} 

pitchanalogvolts := ( pitchadv * (spdpfs-spdmfs)/noc ) 

+ spdmfs; 

pitchrateanalogvolts := ( pitchrateadv * (pitchratepfs - 

pitchratemf s) /noc ) +pitchratemf s ; 



end; { procedure GetDigitalTelemetry } 

procedure ATTITUDE_ ( tdepthvolts, adepthvolts : real; 

var attitude : auvattitude ) ; 

{ This module represents the "AUV Model Reference State 
Space." Actual depth telemetry and the target depth are 
compared and a voltage difference is computed. This 
difference is then "dropped" through a voltage filter 
to determine if the difference if within an acceptable 
tolerance, or if a corrective diveplane command is 
necessary. A "model gain" is applied to the voltage 
difference and an errorvoltage is calculated and passed to 
the main program for dive command generation. Although 
these parameters are single valued, in a multi-state control 
program these parameters could be implemented as arrays and 
the model gain array could be the result of a real- 
time program running synchronously with the main control 
program. } 

{ COMPUTATIONAL SIGN CONVENTION: The voltage difference is 
computed as the difference between TARGET DEPTH , or desired 
AUV depth, and the ACTUAL DEPTH. PLUS voltage DIFFERENCE 
generates down dive plane command; MINUS voltage DIFFERENCE 
generates an UP dive plane command. } 



const 

depthcontroltolerence = 0.1; 

modelgain = 1.0; 

var 

voltsdif ference : real; 



begin { Errorvolts } 

voltsdif ference := tdepthvolts - adepthvolts; 
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{+++++++++++ Control voltage filter +++++++++++++++ } 

{ These conditions check if depth is within tolerence. If 
so a zero error is assigned so as to result in a zero 
diveplane command. } 

if ( voltsdifference > 0) and 

( abs (voltsdifference) <= depthcontroltolerence ) 
then 
begin 

attitude := maintain; 

end 

else if ( voltsdifference < 0) and 

( abs (voltsdifference) <= depthcontroltolerence ) 

then 

begin 

attitude := maintain; 

end 

{ This condition checks if actual depth is less than target 
+ tolerence. In this case a DIVE command is necessary to 
correct depth . } 

else if ( voltsdifference > 0) and 

( abs (voltsdifference) > depthcontroltolerence ) 

then 

begin 

attitude := diving; 

end 

{ This last condition checks to see if the actual depth is 
more than target + tolerence. In this case climb command is 
necessary to correct depth. ) 

else if ( voltsdifference < 0) and 

( abs (voltsdifference) > depthcontroltolerence ) 

then 

begin 

attitude := climb; 
end; 



end; { Errorvolts ) 

procedure EST(var up,yq, z : real ;var vhat: real; var initial: 
integer; var bias,al,a2,a3, a4,a5,a6: real) ; 

label 3 ; 

type 

try = array [1.. 10] of real; 
trytry = array [1. .10,1. .10] of real; 
try40 = array[l. .40,1. .40] of real; 
tryt40 = array [1.. 40] of real; 
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array20 = array[1..20] of real; 
matrix20 = array[l. . 20, 1. . 20] of real; 

const 

ns = 1; 



var 

beta,khat, fc,xhat,g3 ,k,xu,xy,xz,xs,xnew / phi: try 
spt,f3,sq : trytry; 

nx,np,ny,nf ,nsnf ,time, i, j : integer; 
uf,yf,y,yhat,ud,u,yd,pO,sr,sigmax, fs: real; 

procedure initializeArrays; 

var 

i , j : integer ; 
begin 

for i:= 1 to 10 do 
begin 

beta[i] :=0. 0; 
khat[i] :=0. 0; 
xhat [ i ] : = 0.0; 
xu[i] :=0.0; 
xy [i] :=0. 0 ; 
xz[i] :=0.0; 
xnew[ i] : =0 . 0 ; 
xs[i] :=0. 0 ; 
end; 

np := 2*ns; 
nx := np + 1; 
ny := 1; 
y := 0.0; 
uf:= 0.0; 
ud:=0. 0; 
yd:=0. 0; 
yf := 0.0; 
time := 0; 
sigmax ;= 0.10; 
sr ;= 1.0; 
pO : = 1.0e6; 
nf := 1; 
bias := 0.0; 
fc[l] := 0.8; 
fc[2] := 0.2; 
vhat : = 2.1; 
fs ;= 20.0; 
for i : = l to nx do 
begin 

for j:= 1 to nx do 
begin 

phi[i] ;= 0.0; 
sq [ i , j ] : = 0.0; 
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end; 

end; 



spt [ i , j ] ; = 0.0; 

if i = j then 



begin 

sq [ i r j ] := sigmax; 
spt[i,j]:= p0; 
end; 



end; 



procedure innerprod(var y9; real;var phi9 ,x9 : try ;var 

nx9 : integer) ; 



var 

j : integer ; 

begin 
y9 := 0.0; 

for j := 1 to nx9 DO 
begin 

y9 := phi9[j] * x9[j]+ y9; 

end; 

end; 



procedure update (var phi8: try;var u8,y8: real; 
ns: integer) ; 



var 

n2 : integer; 
i : integer ; 
phisave: try; 



begin 

n2 :=ns*2+l; 
for i:=l to 2 do 
begin 

phisave [ i ] : =phi8 [ i ] ; 
end; 

i : = ns; 
while i >= 2 do 
begin 

phi8[i]:= phisave [i - 1]; 
phi8[ns + i] := phisave[ns+i-l] ; 

l := l - 1; 

end; 

phi8 [ 1] := y8 ; 
phi8[ns +1] := u8; 

end; 
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procedure filter (var u4 ,y4 : real ;var x4 , fc4 : try ;var 
nf4 : integer) ; 
var 

nf42 : integer; 
savex4 : try; 

begin 

update (x4,u4,y4,nf 4) ; 
nf42:= 2 * nf 4 ; 
innerprod (y4 ,x4 , fc4 ,nf42) ; 

end; 



procedure newest (var nx7 : integer ; var xhat7 , khat7 , h7 

try; var y7:real) ; 

var 

delta: real; 
i,j: integer; 
xsave : try ; 

begin 

for i:=l to nx7 do 
begin 

xsave [ i ] : =xhat7 [ i ] ; 
end; 

delta := y7; 

for j:= 1 to nx7 do 
begin 

delta := delta - h7[j] * xhat7[j]; 
end; 

for i:= 1 to nx7 do 
begin 

xhat7[i] := xsave [i] + khat7[i] * 
delta; 

end; 

end; 

procedure kgain (nx,ny : integer; var p,q: trytry; r: real; 
var phi,khat: try) ; 



var 

i, j /k 

PPhi, 

phitp 

kden 



: integer; 

: matrix20; 
: array20; 
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begin 

for k:=l to ny do begin 
kden[k] := 0; 
for i:=l to nx do begin 
pphi [i,k] := 0; 
phitp[k, i] := 0; 
for j:=l to nx do begin 

PPhi[i,k] := pphi[i,k] + p[i, j ]*phi[ j ] ; 

phitp[k, i] := phitp[k,i] + phi[ j ] *p[ j , i] ; 
kden[k] := kden[k] + p[i, j ]*phi[ j ]*phi[i] ; 

end; 

end; 

kden[k] := kden[k] + r; 

end; 

for k:=l to ny do begin 

for i:=l to nx do begin 
for j:=l to nx do 

P[i/j] := P[if j ] -PPhi[i,k]*phitp[k, j] 
/kden[k] + q[i,j]; 
khat[i] ;= pphi[i,k]/kden[k] ; 

end; 

end; 

end; 



{ BEGINNING OF PARAMETER ESTIMATION ROUTINE } 



begin 

if initial = 1 
then 

begin 

initializeArrays ; 
goto 3 ; 

end; 
ud ; =uf ; 

filter (up, uf,xu, fc,nf) ; 
yd ;= yf; 

filter (yq, yf , xy , fc, nf ) ; 
update (phi, ud, yd, ns) ; 
phi[nx] :=1.0; 
nsnf := ns + nf; 
time:=time+l; 

if time <= nsnf then goto 3 ; 
innerprod(yhat,phi,xhat,nx) ; 

{ ESTIMATE BETA TERM IN THETA VECTOR } 

kgain(nx,ny,spt,sq,sr,phi,khat) ; 
newest (nx, xhat,khat, phi, yf) ; 
a4:= xhat[l]; a5:= xhat[2]; a6:= xhat[3]; 
(COMPUTE BIAS ESTIMATE FROM THE BETA TERM ) 
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beta[l] :=bias; 
bias : =xhat [ 3 ] ; 

bias:= bias + xhat [ 1 ] *beta [ 1 ] ; 

{ BEGINNING OF ESTIMATER FOR PITCH ANGLE } 
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f3 [1, 1] := 
f 3 [ 1 f 2 ] : = 
f 3 [2 , 1] := 
f 3 [ 2 , 2 ] := 
g3 [1] := 

g3[2] := 

k[l] 

k[2] := 

xnew [ 1 ] : = 

for i := 



1 . 0 ; 

0 . 0 ; 

-vhat/fs; 

1 . 0 ; 

1.0/fs; 

0 . 0 ; 

-12.343/fs; 
7.2/fs; 
yf - bias; 

1 to 2 do 



{subtract bias from pitchrate) 



begin 
j := i+l; 

xnew[j] := f3[i,l]*xs[2] + f 3 [ i, 2 ] *xs[ 3 ] + 
g3 [ i] *xs [ 1] + k[i]*(z - xs[3])); 



end; 

xs[l] := xnew [ 1 ] ; 
xs [ 2 ] ;= xnew [2 ] ; 

xs[3] := xnew[3]; 

al := xs[l];a2 := xs[2];a3 := xs[3]; 
rend; 



procedure GenerateDiveplaneCommand (var 

depth, auvpitch,auvpitchrate, auvdepthcom, delta : real ?var 

k: integer ) ; 

{ This procedure takes digitized voltage values of 
depth, pitch, pitchrate, and target depth in volys and sends 
new commands for control . } 

const 

{ DT 2801— A DIGITAL TO ANALOG Conversion declarations } 

d2achannel0 =0; 
pfs = 10.0; 
mfs = - 10.0; 
noc = 4096; 
cl = 0.5556; 
c2 =1.0; 
c3 = -0.2143; 
a2 = 0.32; 

scalefact = 2.0; 
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var 

digitaldatavalueO , digitaldatavaluel , status 
el , e2 , e3 , vdelta , deltaO , sigma 

function ConvertAnalog2 Digital ( analogvalue : real 

integer ; 

{ This function converts analog signal volts to an 
equivalent digital value. See App D of PCLAB book. 

var 

temp : real ; 

begin 

temp := ( analogvalue - mfs ) * ( (noc - 10) / 

mfs ) ) ; 

convertanalog2digital := round ( temp ) ; 



end; 



function Sat (sig: real) ; real; 

const 

magmax = 1.0; 
begin 

if (abs(sig) <= magmax) then 
begin 

Sat := sig; 
end 

else if (sig > magmax) then 
begin 

Sat : = 1.0; 
end 
else 
begin 

Sat := -1.0; 
end; 

end; 

begin { GenerateDivePlaneCommand - 



e3 := (auvdepthcom-depth) ; 
e2:=(auvpitch) ; 
el:=(auvpitchrate) ; 

sigma := (cl*el) + (c2*e2) + (c3*e3) ; 
deltaO := - (scalef act) * (sat (sigma) ) ; 
delta := deltaO - a2 * e2; 
if k <= 10 then delta:=0.0; 
if (abs (delta) >0 . 4 ) then 
begin 



integer; 
real ; 

): 



(pfs - 
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delta :=0 . 4*abs (delta) /delta; 
end; 

vdelta :=-10. 0*delta; 

digitaldatavalueO := convertanalog2digital (vdelta) ; 
status ;= dacvalue ( d2achannel0, digitaldatavalueO) 



end; 



Genera teDivePlaneCommand 



} 



procedure Initializeparameters ; 

{ This procedure initializes all declared control and 
display parameters to zero.) 

begin { procedure InitializeParameters ) 

yawrate : = 0.0; 
auvdepthvolts : = 0.0; 
auvspeedvolts : = 0.0; 
auvpitchratevolts ;= 0.0; 
auvdepth ; = 0.0; 
auvspeed ; = 0.0; 
auvpitchrate := 0.0; 
estdepth : =0 . 0 ; 
err;=0. 0; 

targe tdepth : = 0.0; 
diveplane ;= 0.0; 

end; { procedure InitializeParameters ) 



begin ( ActiveControl ) 

initializeparameters ; 

initial := 1; ( initial izeArr ays in EST procedure) 

5: 

time : = 1 ; 
clrscr ; 

writeln ( ' DATA FILE NAME? [ie f082201.dat]'); 

readln( filename) ; 

assign (filevar, filename) ; 

rewrite (filevar) ; 

clrscr; 

activemode := run; 

repeat { Repeat until activemode = exit } 



{ ClosedLoopControlScreen; ) 

GetTargetDepth ( targetdepth, targetdepthvolts ) ; 
RunModeScreen ; 

1 : 

while ( not keyhit ( modereply, modereply2)) do 
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begin 

updatecounter := 0; 

while ( updatecounter < updateincrement ) do 
begin 

GetDigitalSensoryData ( auvdepthvolts , yawrate , 

auvpitchratevolts) ; 

auvdepth : = auvdepthvolts ; 
auvpitch : = auvpitchvolts/10 . 0 ; 
auvpitchrate := auvpitchratevolts/ 10. 0; 

GenerateDiveplaneConmiand ( auvdepth , ac2 ,acl, 

targetdepth, diveplane, time ) ; 

EST (diveplane, auvpitchrate, auvdepth, 

speed , initial , biasl , acl , ac2 , ac3 , ac4 , ac5 , ac6) ; 

writeln (f ilevar , time: 5, auvdepth: 12 : 6, speed: 12:6, 

pitchrate : 12 : 6 , diveplane : 12 : 6 , targetdepth : 12 : 6 , 
biasl : 12 : 6 , acl : 12 : 6 , ac2 : 12 : 6 , ac3 : 12 : 6 , ac4 : 12 : 6 , 
ac5:12:6,ac6:12:6) ; 

ATTITUDE_ ( targetdepthvolts , auvdepthvolts , attitude ) ; 
initial := 0; 

updatecounter : = updatecounter + 1 ; 
time := time + 1; 

end; { while updatecounter < updateincrement) 



UpdateRunModeScreen (ac3 ,ac2, acl, diveplane, 
targetdepth , activemode , 

attitude, biasl) ; 



end; { while not KeyHit ) 

if (ord (modereply) = 27) and (ord(modereply2) = 59) 
then 

begin 

close (f ilevar) ; 

goto 5; 
end 

else if (ord (modereply) = 27) and (ord(modereply2)=61) 
then 
begin 

close (f ilevar) ; 
activemode := exit; 

end 

until (activemode=exit) 

end; ( ActiveControl } 
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begin { StatusAndCommand } 

repeat 

StatusAndCommandScreen ; 

GetKey ( ' ' , mode , mode2 ) ; 

if ( ord (mode) = 27 ) and ( ord (mode2) = 59 ) then 
begin 

clrbox2 (xl , yl , x2 , y2 ) ; 

ClosedLoopControl ; 

end; 

until ( mode in ['Q'/'q'] ); 
end; { StatusAndCommand } 



procedure InitializeZeroDigitalSignalOut ; 

{ This procedure MUST be executed as the first procedure 
called in the main program to insure a zero signal out on 
the 2 output channels. Otherwise the DT 2801-A board 
defaults to a minimum full scale output. ) 

const 

digitalchanO = 0 ; 

digitalchanl = 1 ; 

digitalcommandboard = 1; 

var 

status , 

digitaldatavalue : integer; 

begin 

digitaldatavalue := 2048; { This will be converted 

to an equivalent zero analog 
signal out on a 12 bit 
resolution converter like DT 
2801-A. } 

status := initialize; 

status := selectboard ( digitalcommandboard ) ; 
status := dacvalue ( digitalchanO, digitaldatavalue ) ; 
status := dacvalue ( digitalchanl, digitaldatavalue ) ; 
status := terminate; 
end; 

procedure DeactivateADBoardAndExitProgram ; 

{ This procedure deactivates the DT 2801-A board and 
presents an exit screen.} 
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status : integer; 

begin { DeactivateADBoardAndExitProgram 



status := terminate; 
clrbox2 (xl,yl,x2 ,y2) ; 

boxprint (yl+10, xl,x2, 'THIS CONCLUDES YOUR AUV 
AUTOPILOTTING SESSION , BYE 7 ); 

end; { DeactivateADBoardAndExitProgram — 



BEGIN { MAIN PROGRAM 

InitializeZeroDigitalSignalOut; 

clrscr; 

repeat 

MainMenu ( option ) ; 

if ( option in [ 7 R 7 , 7 r 7 ]) then 
begin 

repeat 

begin 

StatusAndCommand ( controlmode ) 
end; 

until ( controlmode in [ 7 q 7 , 7 Q 7 ]); 
end ; 

until ( option in [ 7 Q 7 , 7 q 7 ]); 
DeactivateADBoardAndExitProgram ; 



END. { 



MAIN PROGRAM 
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